/*
 * dist_emulator.c
 *
 *  Created on: Dec 9, 2012
 *      Author: andrey
 */

#include <ch.h>
#include "dist_emulator.h"
#include "io.h"

static volatile int deRpm = 600;//DE_REVOLUTION_PERIOD; // distributor RPM is camshaft RPM
static volatile int deEnabled = TRUE;

//#define LOOPS_TIME(value, divider) (((int)((value) / (divider))) * (divider))

myfloat floatmod(myfloat value, myfloat divider) {
	// number of full cycles
	int loopsNumber = value / divider;
	// total time of full cycles
	myfloat loopsTime = loopsNumber * divider;

	return value - loopsTime;
}

/**
 * @param waveLen length of each wave, in ticks
 */
int waveState(time_t start, time_t now, myfloat waveLen, myfloat leftSide,
		myfloat rightSide) {
	/**
	 * number of ticks since start of thread
	 */
	myfloat time = now - start;

	myfloat mod = floatmod(time, waveLen);

	return mod >= waveLen * leftSide && mod < waveLen * rightSide;
}


static msg_t deThread(void *arg) {
	chRegSetThreadName("Wave");

	myfloat gRpm = deRpm / 60.0; // per minute converted to per second

	myfloat distrGRight = 1.0 / 3;
	myfloat distrRight = 1.0 / 6;

	time_t start_time = chTimeNow();

	while (TRUE) {

		time_t now = chTimeNow();


		int distrG = waveState(start_time, now, freq2wave(4 * gRpm), 0,
				distrGRight);
		// not sure if this is right or should be inverted
		int distr = waveState(start_time, now, freq2wave(gRpm), 0, distrRight);

		setPinValue(DE_IO_PORT, DE_WHITE_WIRE_PIN, deEnabled && distrG);
		setPinValue(DE_IO_PORT, DE_Y_BL_WIRE_PIN, !distr);

		chThdSleep(1);

	}

}

static WORKING_AREA(deThreadStack, 256);

void setRevolutionPeriod(int value) {
	deRpm = value;
}

int getDEEnabled() {
	return deEnabled;
}

void setDEEnabled(int value) {
	deEnabled = value;
}

int getRevolutionPeriod() {
	return deRpm;
}

void initDistributorEmulator() {
	palSetGroupMode(DE_IO_PORT,
			PAL_PORT_BIT(DE_WHITE_WIRE_PIN) | PAL_PORT_BIT(DE_Y_BL_WIRE_PIN), 0,
			PAL_MODE_OUTPUT_PUSHPULL);

	chThdCreateStatic(deThreadStack, sizeof(deThreadStack), NORMALPRIO,
			deThread, NULL );

}
